#ifndef OBSERVER_H
#define OBSERVER_H

#include "Logger.h"
#include "RotationDetector.h"

#include <atomic>
#include <condition_variable>
#include <mutex>
#include <ostream>
#include <queue>

#define DEBUG

// Observer 类负责处理装甲板的跟踪与运动状态推断，支持线程安全的数据接收与状态判断。
// 调度方式如下：
// 1. 主线程通过 tryAddPitchYaw() 向 pitch_yaw_history 队列中添加观测值。
// 2. 当队列满时，触发条件变量 judge_cv 唤醒判定线程（由 runJudgeLoop() 驱动）。
// 3. 判定线程等待条件变量触发后运行状态判断逻辑
// 4. 所有共享资源（队列、状态等）通过 mutex 加锁访问，确保线程安全。
// 5. 标志位 is_judging_motion 为原子变量，避免数据竞争。

class Observer
{
  public:
    Observer();
    ~Observer();

    // 线程安全地获取当前状态
    MotionDetector::MotionStatus getStatus() const;

    // 非阻塞地尝试向 pitch_yaw 队列添加观测数据
    // 若当前处于判定中，或队列已满，则返回 false
    bool tryAddYaw(float yaw);

    // 清空 pitch_yaw 历史队列
    bool ClearYawQueue();

    // 后台判定线程主循环函数，应由外部线程调用（如主函数中）
    void runJudgeLoop();

    // 线程安全地获取车辆中心点坐标
    cv::Point3f getCenterCoordinate() const;

    // 新增：预测中心坐标
    void predictCenterCoordinate(const cv::Point3f &center, float yaw, float w);

  protected:
    // 用于估计 center 的 Kalman 滤波器
    cv::KalmanFilter center_coordinate_kf;

    // 条件变量：在 pitch_yaw_history 满时唤醒判定线程
    std::condition_variable judge_cv;

    // 与 judge_cv 配套的互斥锁
    std::mutex judge_mutex;

    // 判定线程的触发标志（由主线程置 true）
    bool queue_is_full;

    // 当前系统状态，需加锁访问
    MotionDetector::MotionStatus status;
    mutable std::mutex status_mutex;

    // 存储最近的 yaw 观测数据
    std::queue<float> pitch_yaw_history;
    mutable std::mutex pitch_yaw_mutex;

    // 预测的中心点坐标
    cv::Point3f center_coordinate;
    mutable std::mutex center_coordinate_mutex;

    // 表示是否处于状态判断中（由外部控制），为原子变量以避免竞态
    std::atomic<bool> is_judging_motion;

    Logger logger;

    int max_pitch_yaw_history = 20;

    RotationDetector rotationDetector;
};

#endif // OBSERVER_H